/**
 * Tracking the color/shapes
 * Calculate their relative vectors
 */

#include <iostream>
#include "ImageProc.h"
#include "UsbCdcDevice.h"

using namespace std;

/* main code */
int main(int argc, char** argv)
{
    int key = 0;
	int idx = 0;

	UsbCdcDeviceC::instance()->init();
	ImageProcC::instance()->init();
	UsbCdcDeviceC::instance()->postMessage(USB_DCD_MSG_INIT);
	UsbCdcDeviceC::instance()->postMessage(USB_DCD_MSG_UPDATE);

    while (key != 'q') 
	{
		ImageProcC::instance()->update();
		UsbCdcDeviceC::instance()->postMessage(USB_DCD_MSG_UPDATE);
		if (idx == 0)
		{
			UsbCdcDeviceC::instance()->postMessage(USB_DCD_MSG_UPDATE);
		}
		if (idx == 1)
		{
			UsbCdcDeviceC::instance()->postMessage(USB_DCD_MSG_LEFT);
		}
		if (idx == 2)
		{
			UsbCdcDeviceC::instance()->postMessage(USB_DCD_MSG_RIGHT);
		}
		else if (idx > 2)
		{
			idx = -1;
		}
		idx++;
        /* exit if user press 'q' */
        key = cvWaitKey(1);
    }
     
	ImageProcC::instance()->close();
	UsbCdcDeviceC::instance()->close();
       
    return 0;
}
